/***************************************************************************
 创建者: 华磊
 开始时间: 2020.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2020.5.8 类的具体实现代码编写

 ***************************************************************************/
#include "utcalculate.h"
#include "coordinatemanager.h"
#include <QDebug>

UtCalculate::UtCalculate(MotionMessage *motionMessageIn, int robotIdIn, int internalNumIn, CoordinateManager *coordinateManagerIn)
{
    motionMessage=motionMessageIn;
    robotId=robotIdIn;
    internalNum=internalNumIn;
    coordinateManager=coordinateManagerIn;

}


int UtCalculate::setToolCoordinate(ToolCoordinateInfo_frame& toolCoordinate
                                   ,QVector<ToolCoordinateInfo_frame>& toolCoordinateList)
{

    if((toolCoordinate.index>=COORDINATE_MAX_SIZE)||
            (0>=toolCoordinate.index))//设定工具坐标系超出范围
    {
        qDebug()<<"setToolCoordinate error, (toolCoordinate.index>=COORDINATE_MAX_SIZE)||(0>=toolCoordinate.index)";
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "UtCalcute", 7200, robotId);
        return -1;
    }
    toolCoordinateList[toolCoordinate.index].pointList = toolCoordinate.pointList;
    switch(toolCoordinate.method)//工具坐标系设置方法
    {
        case ENUM_TOOL_6POINTS://六点法设定工具坐标系
        {
            if(setToolCoordinateBy6Points(toolCoordinate)<0)//设定失败
            {
                return -2;
            }
            break;
        }
        case ENUM_TOOL_3POINTS://三点法
        {
            if(setToolCoordinateBy3Points(toolCoordinate)<0)//设定失败
            {
                return -2;
            }
            break;
        }
    case ENUM_TOOL_20POINTS_ZEROCAL:
    case ENUM_TOOL_20POINTS:
    {
        if(setToolCoordinateBy20Points(toolCoordinate)<0)
        {
            toolCoordinateList[toolCoordinate.index] = toolCoordinate;
            return -2;
        }
        break;
    }
    case ENUM_TOOL_23POINTS:
    {
        if(setToolCoordinateBy23Points(toolCoordinate)<0)
        {
            toolCoordinateList[toolCoordinate.index] = toolCoordinate;
            return -2;
        }
        break;
    }
        case ENUM_TOOL_XYZWPR://直接法
        {
            if(1!=setToolCoordinateByXyzwpr(toolCoordinate))
            {
                addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "UtCalcute", 7211, robotId);
                return -4;
            }
            break;
        }
        default://不支持工具坐标系设定方法
        {
            addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "UtCalcute", 7201, robotId);
            return -3;
        }
    }

    toolCoordinateList[toolCoordinate.index] = toolCoordinate;



    return 1;
}

int UtCalculate::setUserCoordinate(UserCoordinateInfo_frame &userCoordinate, const QVector<ToolCoordinateInfo_frame> &toolCoordinateList
                                   , QVector<UserCoordinateInfo_frame> &userCoordinateList)
{

    if((userCoordinate.index>=COORDINATE_MAX_SIZE)||
            (0>=userCoordinate.index))//设定用户坐标系超出范围
    {
        qDebug()<<"setUserCoordinate error, userCoordinate.index>=COORDINATE_MAX_SIZE)||(0>=userCoordinate.index)";
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "UtCalcute", 7200, robotId);
        return -1;
    }

    userCoordinateList[userCoordinate.index].pointList = userCoordinate.pointList;

    switch(userCoordinate.method)//用户坐标系设置方法
    {
        case ENUM_USER_3POINTS://三点法设定用户坐标系
        {
            int tcpUtValue = userCoordinateList[userCoordinate.index].pointList[0].utValue;
            if((tcpUtValue>=0)&&(tcpUtValue<COORDINATE_MAX_SIZE))
            {
                Frame tcpForward = toolCoordinateList[tcpUtValue].forwardTcpMatrix;
                if(setUserCoordinateBy3Points(userCoordinate, tcpForward)<0)//设定失败
                {
                    return -2;
                }
                break;
            }

            qDebug()<<"McControllerInterpreter::setUserCoordinate====示教点工具坐标号超过范围"<<tcpUtValue<<COORDINATE_MAX_SIZE;
            addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "UtCalcute", 7208, robotId);
            return -2;
        }
        case ENUM_USER_4POINTS://四点法
        {
            int tcpUtValue = userCoordinateList[userCoordinate.index].pointList[0].utValue;
            if((tcpUtValue>=0)&&(tcpUtValue<COORDINATE_MAX_SIZE))
            {
                Frame tcpForward = toolCoordinateList[tcpUtValue].forwardTcpMatrix;
                if(setUserCoordinateBy4Points(userCoordinate, tcpForward)<0)//设定失败
                {
                    return -2;
                }
                break;
            }
            if(IF_DEBUG)
            {
                qDebug()<<"McControllerInterpreter::setUserCoordinate====示教点工具坐标号超过范围"<<tcpUtValue<<COORDINATE_MAX_SIZE;
            }
            addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "UtCalcute", 2023, robotId);
            return -2;
        }
        case ENUM_USER_XYZWPR://直接法
        {
            if(1!=setUserCoordinateByXyzwpr(userCoordinate))
            {
                addMsg(ENUM_MSG_ERROR,"McControllerInterpreter","UtCalcute",7212,robotId);
                return -4;
            }
            break;
        }
        default://不支持用户坐标系设定方法
        {
            addMsg(ENUM_MSG_ERROR,"McControllerInterpreter","UtCalcute",7201,robotId);
            return -3;
        }
    }

    userCoordinateList[userCoordinate.index] = userCoordinate;


    return 1;
}


int UtCalculate::calculateOrientationNew(Vector transPosVector,const PointPro &point1,const PointPro &point2,
                                      const PointPro &point3,ToolCoordinateInfo_frame& initInfo)
{
    Frame f4 = coordinateManager->pointProToFrame_noUT(point1);
    Frame f5 = coordinateManager->pointProToFrame_noUT(point2);
    Frame f6 = coordinateManager->pointProToFrame_noUT(point3);

    f4.p = f4.M * transPosVector + f4.p;
    f5.p = f5.M * transPosVector + f5.p;
    f6.p = f6.M * transPosVector + f6.p;

    Rotation rotMat;
    if(calculateOrientationBy3Points(true,f4.p,f5.p,f6.p,f4.M,rotMat) < 0)//不能得到旋转矩阵
    {
        qDebug()<<"error,,,calculateOrientationBy3Points() < 0";
        return -4;
    }

    initInfo.forwardTcpMatrix.M =  rotMat;//f4.M.Inverse() *
    initInfo.forwardTcpMatrix.p = transPosVector;
//    if(IF_DEBUG)
//    {
//        qDebug() << "Coordinate::setToolCoordinateBy6Points====rotMat" << rotMat.data[0] << rotMat.data[1] << rotMat.data[2] << rotMat.data[3] << rotMat.data[4] <<
//                    rotMat.data[5] << rotMat.data[6] << rotMat.data[7] << rotMat.data[8];
//        double x, y, z;
//        getTaitBryan(rotMat, x,y,z);
//        qDebug() << "Coordinate::setToolCoordinateBy6Points====rotMat" << x << y << z;
//    }
    PointPro xyz = coordinateManager->frameToPointPro_CART(initInfo.forwardTcpMatrix);
    initInfo.xyzwpr = QVector<double>::fromStdVector( xyz.positionValue );
    initInfo.forwardTcpMatrix = coordinateManager->pointProToFrame_noUT(xyz);
    initInfo.inverseTcpMatrix = initInfo.forwardTcpMatrix.Inverse();
    return 1;

}

//int UtCalculate::calculateOrientation(Vector transPosVector,const PointPro &point1,const PointPro &point2,
//                                      const PointPro &point3,ToolCoordinateInfo_frame& initInfo)
//{
//    Frame f4 = coordinateManager->pointProToFrame_noUT(point1);
//    Frame f5 = coordinateManager->pointProToFrame_noUT(point2);
//    Frame f6 = coordinateManager->pointProToFrame_noUT(point3);
//    Matrix3f rf3;
//    f4.p = f4.M * transPosVector + f4.p;
//    f5.p = f5.M * transPosVector + f5.p;
//    f6.p = f6.M * transPosVector + f6.p;
//    rf3 << f4.p.data[0], f5.p.data[0], f6.p.data[0],
//               f4.p.data[1], f5.p.data[1], f6.p.data[1],
//               f4.p.data[2], f5.p.data[2], f6.p.data[2];
//    Rotation rotMat;
//    if(getRotationBy3Points(rf3, rotMat) < 0)//不能得到旋转矩阵
//    {

//        return -4;
//    }

//    initInfo.forwardTcpMatrix.M =  rotMat;//f4.M.Inverse() *
//    initInfo.forwardTcpMatrix.p = transPosVector;
////    if(IF_DEBUG)
////    {
////        qDebug() << "Coordinate::setToolCoordinateBy6Points====rotMat" << rotMat.data[0] << rotMat.data[1] << rotMat.data[2] << rotMat.data[3] << rotMat.data[4] <<
////                    rotMat.data[5] << rotMat.data[6] << rotMat.data[7] << rotMat.data[8];
////        double x, y, z;
////        getTaitBryan(rotMat, x,y,z);
////        qDebug() << "Coordinate::setToolCoordinateBy6Points====rotMat" << x << y << z;
////    }
//    PointPro xyz = coordinateManager->frameToPointPro_CART(initInfo.forwardTcpMatrix);
//    initInfo.xyzwpr = QVector<double>::fromStdVector( xyz.positionValue );
//    initInfo.forwardTcpMatrix = coordinateManager->pointProToFrame_noUT(xyz);
//    initInfo.inverseTcpMatrix = initInfo.forwardTcpMatrix.Inverse();
//    return 1;

//}




int UtCalculate::setToolCoordinateBy6Points(ToolCoordinateInfo_frame& initInfo)
{
    //检查点的数量
    if(initInfo.pointList.size()<6)
    {//点数量不足6个

        qDebug()<<"UtCalculate::setToolCoordinateBy6Points====点数量不足" << initInfo.pointList.size();
        addMsg(ENUM_MSG_ERROR,"McControllerInterpreter","UtCalcute",7202,robotId);
        return -1;
    }
    //检查点的格式
    for(int i=0; i<6; i++)//关节数目是否准确
    {
        if( (initInfo.pointList[i].jointFlag==1)||
                (internalNum != initInfo.pointList[i].positionValue.size()) )
        {//不是关节点或关节数目不正确
                qDebug()<<"UtCalculate::setToolCoordinateBy6Points====不是关节点或关节数目不正确" << i <<
                          initInfo.pointList[i].jointFlag << initInfo.pointList[i].positionValue.size();
                addMsg(ENUM_MSG_ERROR,"McControllerInterpreter","UtCalcute",7203,robotId);
                return -2;
        }
    }

    //1前三个点设定工具坐标系原点
    Frame f1 = coordinateManager->pointProToFrame_noUT(initInfo.pointList[0]);
    Frame f2 = coordinateManager->pointProToFrame_noUT(initInfo.pointList[1]);
    Frame f3 = coordinateManager->pointProToFrame_noUT(initInfo.pointList[2]);
    Vector transPosVector;
    if(getPointBy3Mat(f1, f2, f3, transPosVector) < 0)//不能得到工具坐标原点
    {

        qDebug() << "UtCalculate::setToolCoordinateBy6Points====不能得到工具坐标原点";
        addMsg(ENUM_MSG_ERROR,"McControllerInterpreter","UtCalcute",7204,robotId);
        return -3;
    }
    initInfo.forwardTcpMatrix.p = transPosVector;

    //2后三个点设定旋转
    if(1!=calculateOrientationNew(transPosVector,initInfo.pointList[3],initInfo.pointList[4],initInfo.pointList[5],initInfo))
    {
        addMsg(ENUM_MSG_ERROR,"McControllerInterpreter","UtCalcute",7205,robotId);
        qDebug() << "Coordinate::setToolCoordinateBy6Points====不能得到旋转矩阵";
        return -1;
    }

    //３误差计算
    initInfo.error = 0.0;
    Vector o1 = f1.p + f1.M  * transPosVector;
    Vector o2 = f2.p + f2.M  * transPosVector;
    Vector o3 = f3.p + f3.M  * transPosVector;
    transPosVector = o1 - o2;
    double d = sqrt(transPosVector.data[0] * transPosVector.data[0] + transPosVector.data[1] * transPosVector.data[1] + transPosVector.data[2] * transPosVector.data[2]) * 1000.0;
    if(d>initInfo.error)
    {
        initInfo.error = d;
    }
    transPosVector = o2 - o3;
    d = sqrt(transPosVector.data[0] * transPosVector.data[0] + transPosVector.data[1] * transPosVector.data[1] + transPosVector.data[2] * transPosVector.data[2]) * 1000.0;
    if(d>initInfo.error)
    {
        initInfo.error = d;
    }

    return 1;
}

int UtCalculate::setToolCoordinateBy3Points(ToolCoordinateInfo_frame &initInfo)
{
    //检查点的数量
    if(initInfo.pointList.size()<3)
    {//点数量不足3个
        qDebug()<<"UtCalcute::setToolCoordinateBy3Points====点数量不足" << initInfo.pointList.size();
        addMsg(ENUM_MSG_ERROR,"McControllerInterpreter","UtCalcute",7206,robotId);
        return -1;
    }
    //检查点的格式
    for(int i=0; i<3; i++)//关节数目是否准确
    {
        if( (initInfo.pointList[i].jointFlag==1)||
                (internalNum != initInfo.pointList[i].positionValue.size()) ){//不是关节点或关节数目不正确

            qDebug()<<"UtCalcute::setToolCoordinateBy3Points====不是关节点或关节数目不正确" << i <<
                          initInfo.pointList[i].jointFlag << initInfo.pointList[i].positionValue.size();
            addMsg(ENUM_MSG_ERROR,"McControllerInterpreter","UtCalcute",7203,robotId);
            return -2;
        }
    }

    //三个点设定工具坐标系原点
    Frame f1 = coordinateManager->pointProToFrame_noUT(initInfo.pointList[0]);
    Frame f2 = coordinateManager->pointProToFrame_noUT(initInfo.pointList[1]);
    Frame f3 = coordinateManager->pointProToFrame_noUT(initInfo.pointList[2]);
    Vector v1;
    if(getPointBy3Mat(f1, f2, f3, v1) < 0)//不能得到工具坐标原点
    {
        qDebug() << "Coordinate::setToolCoordinateBy3Points====不能得到工具坐标原点";
        addMsg(ENUM_MSG_ERROR,"McControllerInterpreter","UtCalcute",7204,robotId);
        return -4;
    }

    initInfo.forwardTcpMatrix.M = Rotation::Identity();
    initInfo.inverseTcpMatrix.M = Rotation::Identity();
    initInfo.forwardTcpMatrix.p = v1;
    initInfo.inverseTcpMatrix.p = -v1;
    PointPro xyz = coordinateManager->frameToPointPro_CART(initInfo.forwardTcpMatrix);
    initInfo.xyzwpr =QVector<double>::fromStdVector( xyz.positionValue );
    initInfo.error = 0.0;
    Vector o1 = f1.p + f1.M  * v1;
    Vector o2 = f2.p + f2.M  * v1;
    Vector o3 = f3.p + f3.M  * v1;
    v1 = o1 - o2;
    double d = sqrt(v1.data[0] * v1.data[0] + v1.data[1] * v1.data[1] + v1.data[2] * v1.data[2]) * 1000.0;
    if(d>initInfo.error){
        initInfo.error = d;
    }
    v1 = o2 - o3;
    d = sqrt(v1.data[0] * v1.data[0] + v1.data[1] * v1.data[1] + v1.data[2] * v1.data[2]) * 1000.0;
    if(d>initInfo.error){
        initInfo.error = d;
    }
    return 1;
}

int UtCalculate::setToolCoordinateBy20Points(ToolCoordinateInfo_frame &initInfo)
{
    qDebug()<<"setToolCoordinateBy20Points";
    if(initInfo.pointList.size()<20)
    {
        qDebug()<<"error,,,initInfo.pointList.size()<20";
        return -1;
    }
    QVector<ToolCoordinateInfo_frame> tmpResultList;
    ToolCoordinateInfo_frame tmpToolPointInfo;
    tmpToolPointInfo.pointList.resize(3);
    if(1)//相邻３个点求原点
    {
            for(int i=0;i<17;i++)
            {
                tmpToolPointInfo.pointList[0]=initInfo.pointList[i];
                tmpToolPointInfo.pointList[1]=initInfo.pointList[i+1];
                tmpToolPointInfo.pointList[2]=initInfo.pointList[i+2];
                if(setToolCoordinateBy3Points(tmpToolPointInfo)<0)//设定失败
                {
                    qDebug()<<"error,setToolCoordinateBy20Points i"<<i<<"j"<<i+1<<"k"<<i+2;
                    addMsg(ENUM_MSG_ERROR,"McControllerInterpreter","UtCalcute",7210,robotId,i+1,i+2,i+3);
                    return -2;
                }
                qDebug()<<"i"<<i<<"errordistance 3p error"<<tmpToolPointInfo.error<<"xyz"
                       <<tmpToolPointInfo.forwardTcpMatrix.p.data[0]
                       <<tmpToolPointInfo.forwardTcpMatrix.p.data[1]<<tmpToolPointInfo.forwardTcpMatrix.p.data[2];
                tmpResultList.append(tmpToolPointInfo);
            }
    }
    else//１个i求一个最好结果
    {
        for(int i=0;i<initInfo.pointList.size()-2;i++)
        {
            double tmpErrorMin=10000;
            ToolCoordinateInfo_frame tmpToolMin;
            for(int j=i+1;j<initInfo.pointList.size()-1;j++)
            {
                for(int k=j+1;k<initInfo.pointList.size();k++)
                {
                    tmpToolPointInfo.pointList[0]=initInfo.pointList[i];
                    tmpToolPointInfo.pointList[1]=initInfo.pointList[j];
                    tmpToolPointInfo.pointList[2]=initInfo.pointList[k];
                    if(setToolCoordinateBy3Points(tmpToolPointInfo)<0)//设定失败
                    {
                        qDebug()<<"error,setToolCoordinateBy20Points i"<<i<<"j"<<j<<"k"<<k;
                        addMsg(ENUM_MSG_ERROR,"McControllerInterpreter","UtCalcute",7210,robotId,i+1,j+1,k+1);
                        return -2;
                    }
    //                qDebug()<<"i"<<i<<"j"<<j<<"k"<<k<<"errordistance 3p "<<tmpToolPointInfo.error;
    //                tmpResultList.append(tmpToolPointInfo);
                    if(tmpToolPointInfo.error<tmpErrorMin)
                    {
                        tmpToolMin=tmpToolPointInfo;
                        tmpErrorMin=tmpToolPointInfo.error;
                    }
                }
            }
            tmpResultList.append(tmpToolMin);
    //        qDebug()<<"i"<<i<<"min errordistance  "<<tmpToolMin.error;

        }
    }



    //求平均值
    double tmpSumX=0;
    double tmpSumY=0;
    double tmpSumZ=0;
    for(int i=0;i<tmpResultList.size();i++)
    {
        tmpSumX+=tmpResultList[i].forwardTcpMatrix.p.data[0];
        tmpSumY+=tmpResultList[i].forwardTcpMatrix.p.data[1];
        tmpSumZ+=tmpResultList[i].forwardTcpMatrix.p.data[2];
//        qDebug()<<"xyzwpr i"<<i<<tmpResultList[i].xyzwpr[0]<<tmpResultList[i].xyzwpr[1]<<tmpResultList[i].xyzwpr[2];
    }
    tmpSumX=tmpSumX/tmpResultList.size();
    tmpSumY=tmpSumY/tmpResultList.size();
    tmpSumZ=tmpSumZ/tmpResultList.size();
    qDebug()<<"average "<<tmpSumX<<tmpSumY<<tmpSumZ;

    initInfo.forwardTcpMatrix.M = Rotation::Identity();
    initInfo.inverseTcpMatrix.M = Rotation::Identity();
    initInfo.forwardTcpMatrix.p.data[0] = tmpSumX;
    initInfo.forwardTcpMatrix.p.data[1] = tmpSumY;
    initInfo.forwardTcpMatrix.p.data[2] = tmpSumZ;
    initInfo.inverseTcpMatrix.p.data[0] = -tmpSumX;
    initInfo.inverseTcpMatrix.p.data[1] = -tmpSumY;
    initInfo.inverseTcpMatrix.p.data[2] = -tmpSumZ;
    PointPro xyz = coordinateManager->frameToPointPro_CART(initInfo.forwardTcpMatrix);
    initInfo.xyzwpr =QVector<double>::fromStdVector( xyz.positionValue );
    initInfo.error = 0.0;

    return 1;

}

int UtCalculate::setToolCoordinateBy23Points(ToolCoordinateInfo_frame &initInfo)
{
    qDebug()<<"setToolCoordinateBy23Points";
    if(23!=initInfo.pointList.size())
    {
        qDebug()<<"error,23!=initInfo.pointList.size()";
        return 0;
    }
    int tmpKey=setToolCoordinateBy20Points(initInfo);
    if(1!=tmpKey)
    {
        return tmpKey;
    }

    //2后三个点设定旋转
    if(1!=calculateOrientationNew(initInfo.forwardTcpMatrix.p,initInfo.pointList[20],initInfo.pointList[21],
                               initInfo.pointList[22],initInfo))
    {
        addMsg(ENUM_MSG_ERROR,"McControllerInterpreter","UtCalcute",7205,robotId);
        qDebug() << "Coordinate::setToolCoordinateBy6Points====不能得到旋转矩阵";
        return -1;
    }
    return 1;

}

int UtCalculate::setToolCoordinateByXyzwpr(ToolCoordinateInfo_frame &initInfo)
{
    //直接法使用世界坐标
    PointPro tmp;
    tmp.jointFlag =1;
    tmp.ufValue = 0;
    tmp.utValue = 0;
    tmp.positionValue.resize(6);
    if(6>initInfo.xyzwpr.size())
    {
        qDebug()<<"error,,,6>initInfo.xyzwpr.size()";
        return 0;
    }
    tmp.positionValue = initInfo.xyzwpr.toStdVector();
    initInfo.forwardTcpMatrix = coordinateManager->pointProToFrame_noUT(tmp);
    initInfo.inverseTcpMatrix = initInfo.forwardTcpMatrix.Inverse();
    initInfo.error = 0.0;
    return 1;
}

int UtCalculate::setUserCoordinateBy3Points(UserCoordinateInfo_frame& initInfo, const Frame& tcpForward)
{
    //检查点的数量
    if(initInfo.pointList.size()<3)//点数量不足3个
    {
        qDebug()<<"Coordinate::setUserCoordinateBy3Points====点数量不足3个"<<initInfo.pointList.size();
        addMsg(ENUM_MSG_ERROR,"McControllerInterpreter","UtCalcute",7206,robotId);
        return -1;
    }
    //检查点的格式，1个点是否有3个数
    for(int i=0; i<3; i++)
    {
        if((initInfo.pointList[i].jointFlag==1)||
                (internalNum != initInfo.pointList[i].positionValue.size()))//点格式不对
        {
            qDebug()<<"Coordinate::setUserCoordinateBy3Points====点格式不对"<< i << initInfo.pointList[i].jointFlag << initInfo.pointList[i].positionValue.size();
            addMsg(ENUM_MSG_ERROR,"McControllerInterpreter","UtCalcute",7207,robotId);
            return -2;
        }
    }
    Frame f1 = coordinateManager->pointProToFrame_noUT(initInfo.pointList[0]) * tcpForward;
    Frame f2 = coordinateManager->pointProToFrame_noUT(initInfo.pointList[1]) * tcpForward;
    Frame f3 = coordinateManager->pointProToFrame_noUT(initInfo.pointList[2]) * tcpForward;
    Rotation rotMat;
    if(calculateOrientationBy3Points(false,f1.p,f2.p,f3.p, Rotation::Identity(),rotMat) < 0)
    {//不能得到旋转矩阵
        qDebug()<<"Coordinate::setUserCoordinateBy3Points====不能得到旋转矩阵";
        addMsg(ENUM_MSG_ERROR,"McControllerInterpreter","UtCalcute",7205,robotId);
        return -3;
    }
    initInfo.forwardTcpMatrix.M = rotMat;
    initInfo.forwardTcpMatrix.p = Vector(f1.p.data[0], f1.p.data[1], f1.p.data[2]);
    PointPro xyz = coordinateManager->frameToPointPro_CART(initInfo.forwardTcpMatrix);
    initInfo.xyzwpr = QVector<double>::fromStdVector( xyz.positionValue );
    initInfo.forwardTcpMatrix = coordinateManager->pointProToFrame_noUT(xyz);
    initInfo.inverseTcpMatrix = initInfo.forwardTcpMatrix.Inverse();
    return 1;
}

int UtCalculate::setUserCoordinateBy4Points(UserCoordinateInfo_frame &initInfo, const Frame& tcpForward)
{
    //检查点的数量
    if(initInfo.pointList.size()<4)//点数量不足3个
    {
        qDebug()<<"Coordinate::setUserCoordinateBy3Points====点数量不足3个"<<initInfo.pointList.size();
        addMsg(ENUM_MSG_ERROR,"McControllerInterpreter","UtCalcute",7206,robotId);
        return -1;
    }
    //检查点的格式，1个点是否有3个数
    for(int i=0; i<4; i++)
    {
        if((initInfo.pointList[i].jointFlag==1)||
                (internalNum != initInfo.pointList[i].positionValue.size()))//点格式不对
        {

            qDebug()<<"Coordinate::setUserCoordinateBy3Points====点格式不对"<< i << initInfo.pointList[i].jointFlag << initInfo.pointList[i].positionValue.size();
            addMsg(ENUM_MSG_ERROR,"McControllerInterpreter","UtCalcute",7207,robotId);
            return -2;
        }
    }
    Frame f1 = coordinateManager->pointProToFrame_noUT(initInfo.pointList[0]) * tcpForward;
    Frame f2 = coordinateManager->pointProToFrame_noUT(initInfo.pointList[1]) * tcpForward;
    Frame f3 = coordinateManager->pointProToFrame_noUT(initInfo.pointList[2]) * tcpForward;
    Frame f0 = coordinateManager->pointProToFrame_noUT(initInfo.pointList[3]) * tcpForward;

    Rotation rotMat;
    if(calculateOrientationBy3Points(false,f1.p,f2.p,f3.p, Rotation::Identity(),rotMat)  < 0)
    {//不能得到旋转矩阵
        addMsg(ENUM_MSG_ERROR,"McControllerInterpreter","UtCalcute",7205,robotId);
        qDebug()<<"Coordinate::setUserCoordinateBy3Points====不能得到旋转矩阵";
        return -3;
    }
    initInfo.forwardTcpMatrix.M = rotMat;
    initInfo.forwardTcpMatrix.p = Vector(f0.p.data[0], f0.p.data[1], f0.p.data[2]);
    PointPro xyz = coordinateManager->frameToPointPro_CART(initInfo.forwardTcpMatrix);
    initInfo.xyzwpr = QVector<double>::fromStdVector( xyz.positionValue );
    initInfo.forwardTcpMatrix = coordinateManager->pointProToFrame_noUT(xyz);
    initInfo.inverseTcpMatrix = initInfo.forwardTcpMatrix.Inverse();
    return 1;
}

int UtCalculate::setUserCoordinateByXyzwpr(UserCoordinateInfo_frame &initInfo)
{
    //直接法使用世界坐标
    PointPro tmp;
    tmp.jointFlag =1;
    tmp.ufValue = 0;
    tmp.utValue = 0;
    tmp.positionValue.resize(6);
    if(6>initInfo.xyzwpr.size())
    {
        return 0;
    }
    tmp.positionValue = initInfo.xyzwpr.toStdVector();
    initInfo.forwardTcpMatrix = coordinateManager->pointProToFrame_noUT(tmp);
    initInfo.inverseTcpMatrix = initInfo.forwardTcpMatrix.Inverse();
    return 1;
}


int UtCalculate::getPointBy3Mat(const Frame& T1, const Frame& T2, const Frame& T3, Vector& point)
{
    Matrix3f deltaRr1, deltaRr2, deltaRr3, A;
    MatrixXf deltaR(9, 3), deltaE(9, 1), deltaRtranspose(3, 9), b(3, 1), fresult(4, 1);
    Vector3f deltaEr1, deltaEr2, deltaEr3;

    //deltaRr1 = T2.block(0, 0, 3, 3) - T1.block(0, 0, 3, 3);
    deltaRr1 << T2.M.data[0]-T1.M.data[0], T2.M.data[1]-T1.M.data[1], T2.M.data[2]-T1.M.data[2],
                         T2.M.data[3]-T1.M.data[3], T2.M.data[4]-T1.M.data[4], T2.M.data[5]-T1.M.data[5],
                         T2.M.data[6]-T1.M.data[6], T2.M.data[7]-T1.M.data[7], T2.M.data[8]-T1.M.data[8];
    //deltaRr2 = T3.block(0, 0, 3, 3) - T2.block(0, 0, 3, 3);
    deltaRr2 << T3.M.data[0]-T2.M.data[0], T3.M.data[1]-T2.M.data[1], T3.M.data[2]-T2.M.data[2],
                         T3.M.data[3]-T2.M.data[3], T3.M.data[4]-T2.M.data[4], T3.M.data[5]-T2.M.data[5],
                         T3.M.data[6]-T2.M.data[6], T3.M.data[7]-T2.M.data[7], T3.M.data[8]-T2.M.data[8];
    //deltaRr3 = T3.block(0, 0, 3, 3) - T1.block(0, 0, 3, 3);
    deltaRr3 << T3.M.data[0]-T1.M.data[0], T3.M.data[1]-T1.M.data[1], T3.M.data[2]-T1.M.data[2],
                         T3.M.data[3]-T1.M.data[3], T3.M.data[4]-T1.M.data[4], T3.M.data[5]-T1.M.data[5],
                         T3.M.data[6]-T1.M.data[6], T3.M.data[7]-T1.M.data[7], T3.M.data[8]-T1.M.data[8];
    deltaR << deltaRr1, deltaRr2, deltaRr3;

    //deltaEr1 = T1.block(0, 3, 3, 1) - T2.block(0, 3, 3, 1);
    deltaEr1 << T1.p.data[0]-T2.p.data[0], T1.p.data[1]-T2.p.data[1], T1.p.data[2]-T2.p.data[2];
    //deltaEr2 = T2.block(0, 3, 3, 1) - T3.block(0, 3, 3, 1);
    deltaEr2 << T2.p.data[0]-T3.p.data[0], T2.p.data[1]-T3.p.data[1], T2.p.data[2]-T3.p.data[2];
    //deltaEr3 = T1.block(0, 3, 3, 1) - T3.block(0, 3, 3, 1);
    deltaEr3 << T1.p.data[0]-T3.p.data[0], T1.p.data[1]-T3.p.data[1], T1.p.data[2]-T3.p.data[2];
    deltaE << deltaEr1, deltaEr2, deltaEr3;
    deltaRtranspose = deltaR.transpose();
    A = deltaRtranspose*deltaR;
    //cout << "deltaR:" << endl << deltaR << endl;
    //cout << "deltaE:" << endl << deltaE << endl;
    //cout << "A:" << endl <<A << endl;
    b = deltaRtranspose*deltaE;
    //cout << "b:" << endl << b<< endl;
    //bool invertible;
    //float determinant;

    //A.computeInverseAndDetWithCheck(Ainverse, determinant, invertible);
    //if (invertible)
    //{
    //	fresult.block(0,0,3,1)= Ainverse*b;
    //	//fresult.block(0, 0, 3, 1) = fresult.block(0, 0, 3, 1).lu().solve(b);
    //	fresult(3, 0) = 0;
    //}
    //else
    //{
    //	fresult << -1, -1, -1, -1;
    //}

    fresult.block(0, 0, 3, 1) = A.fullPivLu().solve(b);
    if (!(A*fresult.block(0, 0, 3, 1)).isApprox(b))
        return -1;
    point = Vector(fresult(0,0), fresult(1,0), fresult(2,0));
    return 1;
}


//int UtCalculate::getRotationBy3Points(const Matrix3f& rf3, Rotation& rotMat)
//{
//    MatrixXf Rf1(3, 3), vo(3, 3);
//    RowVector3f ColNorm;
//    Vector3f v1, v2, v3, nv1, nv2, nv3;

//    v1 = rf3.col(1) - rf3.col(0);
//    v2 = rf3.col(2) - rf3.col(0);
//    v3 = v1.cross(v2);

//    vo.col(0) = v1;
//    vo.col(1) = v2;
//    vo.col(2) = v3;

//    if (fabs(vo.determinant()) < 1e-7){
//        return -1;
//    }
//    Rf1 = getSchmidtMatrix(vo);
//    ColNorm = Rf1.colwise().norm();
//    nv1 = Rf1.col(0) / ColNorm[0];
//    nv2 = Rf1.col(1) / ColNorm[1];
//    nv3 = Rf1.col(2) / ColNorm[2];

//    rotMat =Rotation(nv1(0), nv2(0), nv3(0),
//                                   nv1(1), nv2(1), nv3(1),
//                                   nv1(2), nv2(2), nv3(2));

//    return 1;
//}

int UtCalculate::calculateOrientationBy3Points(bool isXZ,const Vector &frame1,const Vector & frame2,
                   const Vector & frame3,const Rotation &startMat, Rotation &rotMat)
{
    Matrix3f rf3;
    rf3 << frame1.data[0], frame2.data[0], frame3.data[0],
               frame1.data[1], frame2.data[1], frame3.data[1],
               frame1.data[2], frame2.data[2], frame3.data[2];

    Vector3f vx, vz, vy, nv1, nv2, nv3;
    if(isXZ)
    {
        vx = rf3.col(1) - rf3.col(0);//x向量
        vz = rf3.col(2) - rf3.col(1);//接近z向量
        vy = vx.cross(vz);//y向量
        vz = vy.cross(vx);//矫正z向量
//        vz=-vz;//方向取反
    }
    else//XY
    {
        vx = rf3.col(1) - rf3.col(0);//x向量
        vy = rf3.col(2) - rf3.col(1);//接近y向量
        vz = vx.cross(vy);//z向量
        vy = vx.cross(vz);//矫正y向量
        vy=-vy;//方向取反
    }


    vx.normalize();
    vy.normalize();
    vz.normalize();
    if(isnan(vx.norm())||isnan(vy.norm())||isnan(vz.norm()))
    {
        return -1;
    }

    nv1=vx;
    nv2=vy;
    nv3=vz;


    Rotation tmpTargetRo;
    tmpTargetRo =Rotation(nv1(0), nv2(0), nv3(0),
                                   nv1(1), nv2(1), nv3(1),
                                   nv1(2), nv2(2), nv3(2));
    //tmpTargetRo=rotMat*startMat
//    rotMat=tmpTargetRo*(startMat.Inverse());
    rotMat=(startMat.Inverse())*tmpTargetRo;


    return 1;
}


MatrixXf UtCalculate::getSchmidtMatrix(const MatrixXf& Sd)
{
    int n = Sd.rows();
    int m = Sd.cols();
    MatrixXf Orth(n, m);
    Orth.col(0) = Sd.col(0);
    MatrixXf DeltOrth(n, 1), TempM(n, 1);
    for (int i = 1; i != m; ++i)
    {
        DeltOrth << MatrixXf::Zero(n, 1);
        for (int j = 0; j != i; ++j)
        {
            float aa = Orth.col(j).transpose()*Sd.col(i);
            float bb = Orth.col(j).transpose()*Orth.col(j);
            TempM = aa / bb*Orth.col(j);
            //DeltOrth.col(0) = DeltOrth.col(0) + (Orth.col(j).transpose()*Sd.col(i) )/ (Orth.col(j).transpose()*Orth.col(j))*Orth.col(j);
            DeltOrth.col(0) = DeltOrth.col(0) + TempM.col(0);
        }
        Orth.col(i) = Sd.col(i) - DeltOrth.col(0);
    }
    return Orth;
}


void UtCalculate::addMsg(int messageLevel, string componentName, string messageType, int messageCode, int robotIdIn,
                                     int parameter1, int parameter2, int parameter3, int parameter4)
{
    Message tmpMsg;
    tmpMsg.MessageLevel = messageLevel;
    tmpMsg.componentClassName = componentName;
    tmpMsg.messageCode = messageCode;
    tmpMsg.messageType = messageType;
    tmpMsg.robotId = robotIdIn;

    string infomationStr;

    switch(messageCode)
    {
    case 7212://ENUM_MSG_ERROR
    {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")用户坐标设定失败，参数错误!").toStdString();
        infomationStr = str1;
        break;
    }
    case 7211://ENUM_MSG_ERROR
    {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")工具坐标设定失败，参数错误!").toStdString();
        infomationStr = str1;
        break;
    }

        case 7210://ENUM_MSG_ERROR
        {
            string str1;
            str1 = QObject::tr("机器人(ID:").toStdString() +
                    QString::number(robotIdIn).toStdString() +
                    QObject::tr(")坐标设定失败，请检查P%1点P%2点P%3点!").arg(parameter1)
                    .arg(parameter2).arg(parameter3).toStdString();
            infomationStr = str1;
            break;
        }
        case 7209://ENUM_MSG_ERROR
        {
            string str1;
            str1 = QObject::tr("机器人(ID:").toStdString() +
                    QString::number(robotIdIn).toStdString() +
                    QObject::tr(")坐标设定失败，!").toStdString();
            infomationStr = str1;
            break;
        }
        case 7208://ENUM_MSG_ERROR
        {
            string str1;
            str1 = QObject::tr("机器人(ID:").toStdString() +
                    QString::number(robotIdIn).toStdString() +
                    QObject::tr(")坐标设定失败，示教点工具坐标号超过范围!").toStdString();
            infomationStr = str1;
            break;
        }
        case 7207://ENUM_MSG_ERROR
        {
            string str1;
            str1 = QObject::tr("机器人(ID:").toStdString() +
                    QString::number(robotIdIn).toStdString() +
                    QObject::tr(")坐标设定失败，检查点的格式，1个点是否有3个数!").toStdString();
            infomationStr = str1;
            break;
        }
        case 7206://ENUM_MSG_ERROR
        {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")坐标设定失败，点不足３个!").toStdString();
        infomationStr = str1;
        break;
        }
        case 7205://ENUM_MSG_ERROR
        {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")坐标设定失败，不能得到旋转矩阵!").toStdString();
        infomationStr = str1;
        break;
        }
        case 7204://ENUM_MSG_ERROR
        {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")坐标设定失败，不能得到工具坐标原点!").toStdString();
        infomationStr = str1;
        break;
        }

        case 7203://ENUM_MSG_ERROR
        {
            string str1;
            str1 = QObject::tr("机器人(ID:").toStdString() +
                    QString::number(robotIdIn).toStdString() +
                    QObject::tr(")坐标设定失败，不是关节点或关节数目不正确!").toStdString();
            infomationStr = str1;
            break;
        }
        case 7202://ENUM_MSG_ERROR
        {
            string str1;
            str1 = QObject::tr("机器人(ID:").toStdString() +
                    QString::number(robotIdIn).toStdString() +
                    QObject::tr(")坐标设定失败，点数量不足6个!").toStdString();
            infomationStr = str1;
            break;
        }
        case 7201://ENUM_MSG_ERROR
        {
            string str1;
            str1 = QObject::tr("机器人(ID:").toStdString() +
                    QString::number(robotIdIn).toStdString() +
                    QObject::tr(")不支持工具坐标系设定方法!").toStdString();
            infomationStr = str1;
            break;
        }
        case 7200://ENUM_MSG_ERROR
        {
            string str1;
            str1 = QObject::tr("机器人(ID:").toStdString() +
                    QString::number(robotIdIn).toStdString() +
                    QObject::tr(")设定坐标系号超出范围！").toStdString();
            infomationStr = str1;
            break;
        }
        default:
        {
            qDebug()<<"CoordinateSolver::addMsg====not match error code";
            break;
        }
    }

    tmpMsg.MessageInformation = infomationStr;

    #ifndef MOTION_SERVER_UNIT_TEST
    motionMessage->addMsg(tmpMsg);
    #endif
}
